/*
* GLESonGL implementation
* Version:  1.1
* Copyright (c) 2005  Joachim Pouderoux
* Copyright (C) 2003  David Blythe   All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a
* copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included
* in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL
* DAVID BLYTHE BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN
* AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include "glues.h"

#include <string.h>
#include <math.h>

#define __PI		3.14159265358979323846f

void
__identf(GLfloat m[]) 
{
	memset(m, 0, sizeof m[0]*16);
	m[0] = m[5] = m[10] = m[15] = 1.0f;
}

void 
gluPerspectivef(GLfloat fovy, GLfloat aspect, GLfloat n, GLfloat f)
{
	GLfloat m[4][4];
	GLfloat s, cot, dz = f - n;
	GLfloat rad = fovy/2.f*__PI/180.f;

	s = sin(rad);
	if (dz == 0 || s == 0 || aspect == 0) 
		return;

	cot = cos(rad)/s;

	__identf(&m[0][0]);
	m[0][0] = cot/aspect;
	m[1][1] = cot;
	m[2][2] = -(f + n)/dz;
	m[2][3] = -1.f;
	m[3][2] = -2.f*f*n/dz;
	m[3][3] = 0.f;
	glMultMatrixf(&m[0][0]);
}

void
normalizef(float v[3]) 
{
	float r;

	r = (float)sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
	if (r == 0.0) 
		return;
	r = 1.f/r;

	v[0] *= r;
	v[1] *= r;
	v[2] *= r;
}

void
crossf(float v0[3], float v1[3], float r[3])
{
	r[0] = v0[1]*v1[2] - v0[2]*v1[1];
	r[1] = v0[2]*v1[0] - v0[0]*v1[2];
	r[2] = v0[0]*v1[1] - v0[1]*v1[0];
}

void 
gluLookAtf(GLfloat eyex, GLfloat eyey, GLfloat eyez, GLfloat centerx,
		   GLfloat centery, GLfloat centerz, GLfloat upx, GLfloat upy, GLfloat upz) 
{
	GLfloat forward[3], side[3], up[3];
	GLfloat m[4][4];

	forward[0] = centerx - eyex;
	forward[1] = centery - eyey;
	forward[2] = centerz - eyez;

	normalizef(forward);

	up[0] = upx;
	up[1] = upy;
	up[2] = upz;

	crossf(forward, up, side);

	normalizef(side);
	crossf(side, forward, up);

	__identf(&m[0][0]);
	m[0][0] = side[0];
	m[1][0] = side[1];
	m[2][0] = side[2];

	m[0][1] = up[0];
	m[1][1] = up[1];
	m[2][1] = up[2];

	m[0][2] = -forward[0];
	m[1][2] = -forward[1];
	m[2][2] = -forward[2];

	glMultMatrixf(&m[0][0]);
	glTranslatef(-eyex, -eyey, -eyez);
}

// added 09-06-22
static void __gluMakeIdentityf(GLfloat m[16])
{
	m[0+4*0] = 1; m[0+4*1] = 0; m[0+4*2] = 0; m[0+4*3] = 0;
	m[1+4*0] = 0; m[1+4*1] = 1; m[1+4*2] = 0; m[1+4*3] = 0;
	m[2+4*0] = 0; m[2+4*1] = 0; m[2+4*2] = 1; m[2+4*3] = 0;
	m[3+4*0] = 0; m[3+4*1] = 0; m[3+4*2] = 0; m[3+4*3] = 1;
}

static void __gluMultMatrixVecf(const GLfloat matrix[16], const GLfloat in[4],
								GLfloat out[4])
{
	int i;

	for (i=0; i<4; i++) {
		out[i] = 
			in[0] * matrix[0*4+i] +
			in[1] * matrix[1*4+i] +
			in[2] * matrix[2*4+i] +
			in[3] * matrix[3*4+i];
	}
}

/*
** inverse = invert(src)
*/
static int __gluInvertMatrixf(const GLfloat src[16], GLfloat inverse[16])
{
	int i, j, k, swap;
	double t;
	GLfloat temp[4][4];

	for (i=0; i<4; i++) {
		for (j=0; j<4; j++) {
			temp[i][j] = src[i*4+j];
		}
	}
	__gluMakeIdentityf(inverse);

	for (i = 0; i < 4; i++) {
		/*
		** Look for largest element in column
		*/
		swap = i;
		for (j = i + 1; j < 4; j++) {
			if (fabs(temp[j][i]) > fabs(temp[i][i])) {
				swap = j;
			}
		}

		if (swap != i) {
			/*
			** Swap rows.
			*/
			for (k = 0; k < 4; k++) {
				t = temp[i][k];
				temp[i][k] = temp[swap][k];
				temp[swap][k] = t;

				t = inverse[i*4+k];
				inverse[i*4+k] = inverse[swap*4+k];
				inverse[swap*4+k] = t;
			}
		}

		if (temp[i][i] == 0) {
			/*
			** No non-zero pivot.  The matrix is singular, which shouldn't
			** happen.  This means the user gave us a bad matrix.
			*/
			return GL_FALSE;
		}

		t = temp[i][i];
		for (k = 0; k < 4; k++) {
			temp[i][k] /= t;
			inverse[i*4+k] /= t;
		}
		for (j = 0; j < 4; j++) {
			if (j != i) {
				t = temp[j][i];
				for (k = 0; k < 4; k++) {
					temp[j][k] -= temp[i][k]*t;
					inverse[j*4+k] -= inverse[i*4+k]*t;
				}
			}
		}
	}
	return GL_TRUE;
}

static void __gluMultMatricesf(const GLfloat a[16], const GLfloat b[16],
							   GLfloat r[16])
{
	int i, j;

	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			r[i*4+j] = 
				a[i*4+0]*b[0*4+j] +
				a[i*4+1]*b[1*4+j] +
				a[i*4+2]*b[2*4+j] +
				a[i*4+3]*b[3*4+j];
		}
	}
}

GLint
gluProject(GLfloat objx, GLfloat objy, GLfloat objz, 
		   const GLfloat modelMatrix[16], 
		   const GLfloat projMatrix[16],
		   const GLint viewport[4],
		   GLfloat *winx, GLfloat *winy, GLfloat *winz)
{
	GLfloat in[4];
	GLfloat out[4];

	in[0]=objx;
	in[1]=objy;
	in[2]=objz;
	in[3]=1.0f;
	__gluMultMatrixVecf(modelMatrix, in, out);
	__gluMultMatrixVecf(projMatrix, out, in);
	if (in[3] == 0.0f) return(GL_FALSE);
	in[0] /= in[3];
	in[1] /= in[3];
	in[2] /= in[3];
	/* Map x, y and z to range 0-1 */
	in[0] = in[0] * 0.5f + 0.5f;
	in[1] = in[1] * 0.5f + 0.5f;
	in[2] = in[2] * 0.5f + 0.5f;

	/* Map x,y to viewport */
	in[0] = in[0] * viewport[2] + viewport[0];
	in[1] = in[1] * viewport[3] + viewport[1];

	*winx=in[0];
	*winy=in[1];
	*winz=in[2];
	return(GL_TRUE);
}

GLint
gluUnProject(GLfloat winx, GLfloat winy, GLfloat winz,
			 const GLfloat modelMatrix[16], 
			 const GLfloat projMatrix[16],
			 const GLint viewport[4],
			 GLfloat *objx, GLfloat *objy, GLfloat *objz)
{
	GLfloat finalMatrix[16];
	GLfloat in[4];
	GLfloat out[4];

	__gluMultMatricesf(modelMatrix, projMatrix, finalMatrix);
	if (!__gluInvertMatrixf(finalMatrix, finalMatrix)) return(GL_FALSE);

	in[0]=winx;
	in[1]=winy;
	in[2]=winz;
	in[3]=1.0f;

	/* Map x and y from window coordinates */
	in[0] = (in[0] - viewport[0]) / viewport[2];
	in[1] = (in[1] - viewport[1]) / viewport[3];

	/* Map to range -1 to 1 */
	in[0] = in[0] * 2 - 1;
	in[1] = in[1] * 2 - 1;
	in[2] = in[2] * 2 - 1;

	__gluMultMatrixVecf(finalMatrix, in, out);
	if (out[3] == 0.0f) return(GL_FALSE);
	out[0] /= out[3];
	out[1] /= out[3];
	out[2] /= out[3];
	*objx = out[0];
	*objy = out[1];
	*objz = out[2];
	return(GL_TRUE);
}